/*==============================================================================================*/
/*==============================================================================================*/
#include "stm32f1_system.h"
#include "stm32f1_i2cSim.h"
#include "module_mpu6050.h"
/*==============================================================================================*/
/*==============================================================================================*
**函數 : MPU6050_Init
**功能 : 初始化 MPU6050
**輸入 : None
**輸出 : Status
**使用 : Status = MPU6050_Init();
**==============================================================================================*/
/*==============================================================================================*/
uint8_t MPU6050_Init( void )
{
  uint8_t i = 0;

  uint8_t MPU6050_InitData[6][2] = {
    {0x01, MPU6050_PWR_MGMT_1},   // Reset device
    {0x03, MPU6050_CONFIG},       // 
    {0x18, MPU6050_GYRO_CONFIG},  // +-2000dps
    {0x08, MPU6050_ACCEL_CONFIG}, // +-4G
    {0x32, MPU6050_INT_PIN_CFG},  // 
    {0x00, MPU6050_USER_CTRL}     // 
  };

  SimI2C_Config();
  Delay_10ms(1);

  /* Check Device ID */
  if(MPU6050_GetDeviceID() != MPU6050_Device_ID)
    return ERROR;

  Delay_10ms(1);

  for(i = 0; i < 6; i++) {
    SimI2C_WriteReg(MPU6050_I2C_ADDR, MPU6050_InitData[i][1], MPU6050_InitData[i][0]);
    Delay_10ms(1);
  }

  return SUCCESS;
}
/*====================================================================================================*/
/*====================================================================================================*
**函數 : MPU6050_GetDeviceID
**功能 : MPU6050 Get Device ID
**輸入 : None
**輸出 : DeviceID
**使用 : DeviceID = MPU6050_GetDeviceID();
**====================================================================================================*/
/*====================================================================================================*/
uint8_t MPU6050_GetDeviceID( void )
{
	uint8_t DeviceID = 0x00;

  SimI2C_ReadReg(MPU6050_I2C_ADDR, MPU6050_WHO_AM_I, &DeviceID);

  return DeviceID;
}
/*==============================================================================================*/
/*==============================================================================================*
**函數 : MPU6050_GetData
**功能 : 讀取感測器資料
**輸入 : *ReadBuf
**輸出 : None
**使用 : MPU6050_GetData(IMU_Buf);
**==============================================================================================*/
/*==============================================================================================*/
void MPU6050_GetData( int16_t *IMU_Buf )
{
  uint8_t ReadBuf[16] = {0};

  SimI2C_ReadRegs(MPU6050_I2C_ADDR, MPU6050_ACCEL_XOUT_H, ReadBuf, 14);

  IMU_Buf[0] = (Byte16(int16_t, ReadBuf[0],  ReadBuf[1]));    // Acc.X
  IMU_Buf[1] = (Byte16(int16_t, ReadBuf[2],  ReadBuf[3]));    // Acc.Y
  IMU_Buf[2] = (Byte16(int16_t, ReadBuf[4],  ReadBuf[5]));    // Acc.Z
  IMU_Buf[3] = (Byte16(int16_t, ReadBuf[8],  ReadBuf[9]));    // Gyr.X
  IMU_Buf[4] = (Byte16(int16_t, ReadBuf[10], ReadBuf[11]));   // Gyr.Y
  IMU_Buf[5] = (Byte16(int16_t, ReadBuf[12], ReadBuf[13]));   // Gyr.Z
  IMU_Buf[6] = (Byte16(int16_t, ReadBuf[6],  ReadBuf[7]));    // Temp
}
/*==============================================================================================*/
/*==============================================================================================*/

